home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 60 / IOPROG_60.ISO / soft / c++ / gsl-1.1.1-setup.exe / {app} / src / multiroots / dogleg.c < prev    next >
Encoding:
C/C++ Source or Header  |  2000-10-21  |  9.5 KB  |  414 lines
  1. /* multiroots/dogleg.c
  2.  * 
  3.  * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
  4.  * 
  5.  * This program is free software; you can redistribute it and/or modify
  6.  * it under the terms of the GNU General Public License as published by
  7.  * the Free Software Foundation; either version 2 of the License, or (at
  8.  * your option) any later version.
  9.  * 
  10.  * This program is distributed in the hope that it will be useful, but
  11.  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.  * General Public License for more details.
  14.  * 
  15.  * You should have received a copy of the GNU General Public License
  16.  * along with this program; if not, write to the Free Software
  17.  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18.  */
  19.  
  20. #include "enorm.c"
  21.  
  22. static void compute_diag (const gsl_matrix * J, gsl_vector * diag);
  23. static void update_diag (const gsl_matrix * J, gsl_vector * diag);
  24. static double compute_delta (gsl_vector * diag, gsl_vector * x);
  25. static void compute_df (const gsl_vector * f_trial, const gsl_vector * f, gsl_vector * df);
  26. static void compute_wv (const gsl_vector * qtdf, const gsl_vector *rdx, const gsl_vector *dx, const gsl_vector *diag, double pnorm, gsl_vector * w, gsl_vector * v);
  27.  
  28. static double scaled_enorm (const gsl_vector * d, const gsl_vector * f);
  29.  
  30. static double scaled_enorm (const gsl_vector * d, const gsl_vector * f) {
  31.   double e2 = 0 ;
  32.   size_t i, n = f->size ;
  33.   for (i = 0; i < n ; i++) {
  34.     double fi= gsl_vector_get(f, i);
  35.     double di= gsl_vector_get(d, i);
  36.     double u = di * fi;
  37.     e2 += u * u ;
  38.   }
  39.   return sqrt(e2);
  40. }
  41.  
  42. static double enorm_sum (const gsl_vector * a, const gsl_vector * b);
  43.  
  44. static double enorm_sum (const gsl_vector * a, const gsl_vector * b) {
  45.   double e2 = 0 ;
  46.   size_t i, n = a->size ;
  47.   for (i = 0; i < n ; i++) {
  48.     double ai= gsl_vector_get(a, i);
  49.     double bi= gsl_vector_get(b, i);
  50.     double u = ai + bi;
  51.     e2 += u * u ;
  52.   }
  53.   return sqrt(e2);
  54. }
  55.  
  56. static void
  57. compute_wv (const gsl_vector * qtdf, const gsl_vector *rdx, const gsl_vector *dx, const gsl_vector *diag, double pnorm, gsl_vector * w, gsl_vector * v)
  58. {
  59.   size_t i, n = qtdf->size;
  60.  
  61.   for (i = 0; i < n; i++)
  62.     {
  63.       double qtdfi = gsl_vector_get (qtdf, i);
  64.       double rdxi = gsl_vector_get (rdx, i);
  65.       double dxi = gsl_vector_get (dx, i);
  66.       double diagi = gsl_vector_get (diag, i);
  67.  
  68.       gsl_vector_set (w, i, (qtdfi - rdxi) / pnorm);
  69.       gsl_vector_set (v, i, diagi * diagi * dxi / pnorm);
  70.     }
  71. }
  72.  
  73.  
  74. static void
  75. compute_df (const gsl_vector * f_trial, const gsl_vector * f, gsl_vector * df)
  76. {
  77.   size_t i, n = f->size;
  78.  
  79.   for (i = 0; i < n; i++)
  80.     {
  81.       double dfi = gsl_vector_get (f_trial, i) - gsl_vector_get (f, i);
  82.       gsl_vector_set (df, i, dfi);
  83.     }
  84. }
  85.  
  86. static void
  87. compute_diag (const gsl_matrix * J, gsl_vector * diag)
  88. {
  89.   size_t i, j, n = diag->size;
  90.  
  91.   for (j = 0; j < n; j++)
  92.     {
  93.       double sum = 0;
  94.       for (i = 0; i < n; i++)
  95.     {
  96.       double Jij = gsl_matrix_get (J, i, j);
  97.       sum += Jij * Jij;
  98.     }
  99.       if (sum == 0)
  100.     sum = 1.0;
  101.  
  102.       gsl_vector_set (diag, j, sqrt (sum));
  103.     }
  104. }
  105.  
  106. static void
  107. update_diag (const gsl_matrix * J, gsl_vector * diag)
  108. {
  109.   size_t i, j, n = diag->size;
  110.  
  111.   for (j = 0; j < n; j++)
  112.     {
  113.       double cnorm, diagj, sum = 0;
  114.       for (i = 0; i < n; i++)
  115.     {
  116.       double Jij = gsl_matrix_get (J, i, j);
  117.       sum += Jij * Jij;
  118.     }
  119.       if (sum == 0)
  120.     sum = 1.0;
  121.  
  122.       cnorm = sqrt (sum);
  123.       diagj = gsl_vector_get (diag, j);
  124.  
  125.       if (cnorm > diagj)
  126.     gsl_vector_set (diag, j, cnorm);
  127.     }
  128. }
  129.  
  130. static double
  131. compute_delta (gsl_vector * diag, gsl_vector * x)
  132. {
  133.   double Dx = scaled_enorm (diag, x);
  134.   double factor = 100;
  135.  
  136.   return (Dx > 0) ? factor * Dx : factor;
  137. }
  138.  
  139. static double
  140. compute_actual_reduction (double fnorm, double fnorm1)
  141. {
  142.   double actred;
  143.  
  144.   if (fnorm1 < fnorm)
  145.     {
  146.       double u = fnorm1 / fnorm;
  147.       actred = 1 - u * u;
  148.     }
  149.   else
  150.     {
  151.       actred = -1;
  152.     }
  153.  
  154.   return actred;
  155. }
  156.  
  157. static double
  158. compute_predicted_reduction (double fnorm, double fnorm1)
  159. {
  160.   double prered;
  161.  
  162.   if (fnorm1 < fnorm)
  163.     {
  164.       double u = fnorm1 / fnorm;
  165.       prered = 1 - u * u;
  166.     }
  167.   else
  168.     {
  169.       prered = 0;
  170.     }
  171.  
  172.   return prered;
  173. }
  174.  
  175. static void 
  176. compute_qtf (const gsl_matrix * q, const gsl_vector * f, gsl_vector * qtf)
  177. {
  178.   size_t i, j, N = f->size ;
  179.  
  180.   for (j = 0; j < N; j++)
  181.     {
  182.       double sum = 0;
  183.       for (i = 0; i < N; i++)
  184.     sum += gsl_matrix_get (q, i, j) * gsl_vector_get (f, i);
  185.  
  186.       gsl_vector_set (qtf, j, sum);
  187.     }
  188. }
  189.  
  190. static void 
  191. compute_rdx (const gsl_matrix * r, const gsl_vector * dx, gsl_vector * rdx)
  192. {
  193.   size_t i, j, N = dx->size ;
  194.  
  195.   for (i = 0; i < N; i++)
  196.     {
  197.       double sum = 0;
  198.  
  199.       for (j = i; j < N; j++)
  200.     {
  201.       sum += gsl_matrix_get (r, i, j) * gsl_vector_get (dx, j);
  202.     }
  203.  
  204.       gsl_vector_set (rdx, i, sum);
  205.     }
  206. }
  207.  
  208.  
  209. static void
  210. compute_trial_step (gsl_vector *x, gsl_vector * dx, gsl_vector * x_trial)
  211. {
  212.   size_t i, N = x->size;
  213.  
  214.   for (i = 0; i < N; i++)
  215.     {
  216.       double pi = gsl_vector_get (dx, i);
  217.       double xi = gsl_vector_get (x, i);
  218.       gsl_vector_set (x_trial, i, xi + pi);
  219.     }
  220. }
  221.  
  222. static int
  223. newton_direction (const gsl_matrix * r, const gsl_vector * qtf, gsl_vector * p)
  224. {
  225.   const size_t N = r->size2;
  226.   size_t i;
  227.   int status;
  228.  
  229.   status = gsl_linalg_R_solve (r, qtf, p);
  230.  
  231. #ifdef DEBUG
  232.   printf("rsolve status = %d\n", status);
  233. #endif
  234.  
  235.   for (i = 0; i < N; i++)
  236.     {
  237.       double pi = gsl_vector_get (p, i);
  238.       gsl_vector_set (p, i, -pi);
  239.     }
  240.  
  241.   return status;
  242. }
  243.  
  244. static void
  245. gradient_direction (const gsl_matrix * r, const gsl_vector * qtf,
  246.             const gsl_vector * diag, gsl_vector * g)
  247. {
  248.   const size_t M = r->size1;
  249.   const size_t N = r->size2;
  250.  
  251.   size_t i, j;
  252.  
  253.   for (j = 0; j < M; j++)
  254.     {
  255.       double sum = 0;
  256.       double dj;
  257.  
  258.       for (i = 0; i < N; i++)
  259.     {
  260.       sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
  261.     }
  262.  
  263.       dj = gsl_vector_get (diag, j);
  264.       gsl_vector_set (g, j, -sum / dj);
  265.     }
  266. }
  267.  
  268. static void
  269. minimum_step (double gnorm, const gsl_vector * diag, gsl_vector * g)
  270. {
  271.   const size_t N = g->size;
  272.   size_t i;
  273.  
  274.   for (i = 0; i < N; i++)
  275.     {
  276.       double gi = gsl_vector_get (g, i);
  277.       double di = gsl_vector_get (diag, i);
  278.       gsl_vector_set (g, i, (gi / gnorm) / di);
  279.     }
  280. }
  281.  
  282. static void
  283. compute_Rg (const gsl_matrix * r, const gsl_vector * gradient, gsl_vector * Rg)
  284. {
  285.   const size_t N = r->size2;
  286.   size_t i, j;
  287.  
  288.   for (i = 0; i < N; i++)
  289.     {
  290.       double sum = 0;
  291.  
  292.       for (j = i; j < N; j++)
  293.     {
  294.       double gj = gsl_vector_get (gradient, j);
  295.       double rij = gsl_matrix_get (r, i, j);
  296.       sum += rij * gj;
  297.     }
  298.  
  299.       gsl_vector_set (Rg, i, sum);
  300.     }
  301. }
  302.  
  303. static void
  304. scaled_addition (double alpha, gsl_vector * newton, double beta, gsl_vector * gradient, gsl_vector * p)
  305. {
  306.   const size_t N = p->size;
  307.   size_t i;
  308.  
  309.   for (i = 0; i < N; i++)
  310.     {
  311.       double ni = gsl_vector_get (newton, i);
  312.       double gi = gsl_vector_get (gradient, i);
  313.       gsl_vector_set (p, i, alpha * ni + beta * gi);
  314.     }
  315. }
  316.  
  317. static int
  318. dogleg (const gsl_matrix * r, const gsl_vector * qtf,
  319.     const gsl_vector * diag, double delta,
  320.     gsl_vector * newton, gsl_vector * gradient, gsl_vector * p)
  321. {
  322.   double qnorm, gnorm, sgnorm, bnorm, temp;
  323.  
  324.   newton_direction (r, qtf, newton);
  325.  
  326. #ifdef DEBUG
  327.   printf("newton = "); gsl_vector_fprintf(stdout, newton, "%g"); printf("\n");
  328. #endif
  329.  
  330.   qnorm = scaled_enorm (diag, newton);
  331.  
  332.   if (qnorm <= delta)
  333.     {
  334.       gsl_vector_memcpy (p, newton);
  335. #ifdef DEBUG
  336.       printf("took newton (qnorm = %g  <=   delta = %g)\n", qnorm, delta);
  337. #endif
  338.       return GSL_SUCCESS;
  339.     }
  340.  
  341.   gradient_direction (r, qtf, diag, gradient);
  342.  
  343. #ifdef DEBUG
  344.   printf("grad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
  345. #endif
  346.  
  347.   gnorm = enorm (gradient);
  348.  
  349.   if (gnorm == 0)
  350.     {
  351.       double alpha = delta / qnorm;
  352.       double beta = 0;
  353.       scaled_addition (alpha, newton, beta, gradient, p);
  354. #ifdef DEBUG
  355.       printf("took scaled newton because gnorm = 0\n");
  356. #endif
  357.       return GSL_SUCCESS;
  358.     }
  359.  
  360.   minimum_step (gnorm, diag, gradient);
  361.  
  362.   compute_Rg (r, gradient, p);    /* Use p as temporary space to compute Rg */
  363.  
  364. #ifdef DEBUG
  365.   printf("mingrad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
  366.   printf("Rg = "); gsl_vector_fprintf(stdout, p, "%g"); printf("\n");
  367. #endif
  368.  
  369.   temp = enorm (p);
  370.   sgnorm = (gnorm / temp) / temp;
  371.  
  372.   if (sgnorm > delta)
  373.     {
  374.       double alpha = 0;
  375.       double beta = delta;
  376.       scaled_addition (alpha, newton, beta, gradient, p);
  377. #ifdef DEBUG
  378.       printf("took gradient\n");
  379. #endif
  380.       return GSL_SUCCESS;
  381.     }
  382.  
  383.   bnorm = enorm (qtf);
  384.  
  385.   {
  386.     double bg = bnorm / gnorm;
  387.     double bq = bnorm / qnorm;
  388.     double dq = delta / qnorm;
  389.     double dq2 = dq * dq;
  390.     double sd = sgnorm / delta;
  391.     double sd2 = sd * sd;
  392.  
  393.     double t1 = bg * bq * sd;
  394.     double u = t1 - dq;
  395.     double t2 = t1 - dq * sd2 + sqrt (u * u + (1-dq2) * (1 - sd2));
  396.  
  397.     double alpha = dq * (1 - sd2) / t2;
  398.     double beta = (1 - alpha) * sgnorm;
  399.  
  400. #ifdef DEBUG
  401.     printf("bnorm = %g\n", bnorm);
  402.     printf("gnorm = %g\n", gnorm);
  403.     printf("qnorm = %g\n", qnorm);
  404.     printf("delta = %g\n", delta);
  405.     printf("alpha = %g   beta = %g\n", alpha, beta);
  406.     printf("took scaled combination of newton and gradient\n");
  407. #endif
  408.  
  409.     scaled_addition (alpha, newton, beta, gradient, p);
  410.   }
  411.  
  412.   return GSL_SUCCESS;
  413. }
  414.